
#ifndef ELLIPSOID_H
#define ELLIPSOID_H

#include <iomanip>
#include "Core/Vec.h"
#include "Core/numlib.h"

template <int D>
class Ellipsoid
{
public:
  using rVec = Vec<Real, D>;

  Ellipsoid(rVec aCenter, rVec aRadius)
      : center(aCenter), radius(aRadius)
  {
  }

  rVec getNormalAt(rVec p) const {
    return normalize((p - center) / (radius * radius));
  }

  void check(rVec p, rVec &closestPoint, rVec &normal, bool &contained, Real tol) const
  {
    p = p - center;
    if(norm(p, 2) <= tol * norm(radius, 2)) {
      auto iter = std::min_element(&radius[0], &radius[D]);
      int d = iter - &radius[0];
      closestPoint = center + radius * rVec::unit(d);
      normal = getNormalAt(closestPoint);
      contained = true;
      return;
    }
    // 1. Find an initial guess.
    rVec q = p / radius;
    Real t = 1.0 / sqrt(sum(q * q));
    contained = (t > 1.0);
    rVec one = 1.0;
    auto H = [&](Real z) {
      rVec a = radius - one * z / radius;
      return sum((p*p)/(a*a)) - 1.0;
    };
    Real c0 = ipow<2>(min_of(radius));
    Real c = c0;
    while(H(c0-c) < 0.0)
      c /= 2.0;
    // 2. Find the Lagrangian multiplier lambda.
    auto DH = [&](Real z) {
      rVec a = radius - one * z / radius;
      return sum((p*p)/(a*a*a)/radius) * 2.0;
    };
    Real lambda = fzero(H, DH, c0-c, 10, tol);
    // 3. Recover the closest point.
    closestPoint = p / (one - one * lambda / (radius*radius));
    closestPoint = center + closestPoint;
    normal = getNormalAt(closestPoint);
    //
    rVec dir = p - closestPoint;
    Real rad = std::abs(dot(dir, normal) / (norm(dir, 2) * norm(normal, 2)));
    dbgcout3 << "Ellipsoid::check() radian = "
             << std::scientific << std::setprecision(10)
             << rad << "\n" << std::defaultfloat;
  }

protected:
  rVec center;
  rVec radius;
};

#endif //ELLIPSOID_H
